#include <math.h>
#include "baroalt.h"

/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */

/*
 * PERFORMANCE HINT:
 *
 * The single precision calculation is 50 microseconds faster than the double
 * precision variant. It is however not obvious if double precision is required.
 * Pending more inspection and tests, we'll leave the double precision variant active.
 *
 * Measurements:
 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
 */

float calc_baroalt(double static_pressure, double base_pressure) {
	/* tropospheric properties (0-11km) for standard atmosphere */
	const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
	const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
	const double g  = 9.80665;	/* gravity constant in m/s/s */
	const double R  = 287.05;	/* ideal gas constant in J/kg/K */

	/* pressure at 0m altitude in kPa */
	double p1 = base_pressure / 1000.0;

	/* measured pressure in kPa */
	double p = static_pressure / 1000.0;

	/*
	 * Solve:
	 *
	 *     /        -(aR / g)     \
	 *    | (p / p1)          . T1 | - T1
	 *     \                      /
	 * h = -------------------------------  + h1
	 *                   a
	 */
	return (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
}
